#!/usr/bin/env python

import rospy
from flydatas_msgs.msg import Flydatas
from sensor_msgs.msg import NavSatFix
from mavros_msgs.msg import State
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header

should_publish = True

def flydatas_callback(msg):
    # 处理 GPS 数据
    gps_lat = msg.gps[0].latitude
    gps_lon = msg.gps[0].longitude
    gps_alt = msg.gps[0].altitude
    
    # # 打印 GPS 数据
    # rospy.loginfo("GPS - Latitude: %f, Longitude: %f, Altitude: %f", gps_lat, gps_lon, gps_alt)

    if msg.doit == True:
        global should_publish
        pub.unregister()
        should_publish = False
    # else:
    #     global should_publish
    #     should_publish = True


    


def publish_flydatas():
    # rospy.init_node('flydatas_publisher', anonymous=True)
    # pub = rospy.Publisher('/pub_flydatas', Flydatas, queue_size=10)
    rate = rospy.Rate(10)  # 10 Hz

    msg = Flydatas()

    # Filling Header
    msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "base_link"

    # Filling State (replace with actual values)
    msg.flight_state = State()
    msg.flight_state.mode = "OFFBOARD"
    msg.flight_state.armed = True

    # Filling GPS data (replace with actual GPS values)
    msg.gps = []  # Clear the list to ensure no extra items are present
    for i in range(4):  # Ensure exactly 4 items are added
        gps = NavSatFix()# 初始lat：21.679509，lon：110.918809,alt:16.5
        # gps.latitude = 47.397742 + i * 0.0001  # Example latitudes
        # gps.longitude = 8.545693 + i * 0.0001  # Example longitudes
        # gps.altitude = 550.365917 + i  # Example altitudes
        msg.gps.append(gps)

    msg.gps[0].latitude = 0.5
    msg.gps[0].longitude = 0.0
    msg.gps[0].altitude = 19.5
    # 47°23'51.4"N 8°32'52.2"E
    msg.gps[1].latitude = 0
    msg.gps[1].longitude = 0
    msg.gps[1].altitude = 0
    # 47°23'51.9"N 8°32'44.1"E
    msg.gps[2].latitude = 0
    msg.gps[2].longitude =  0
    msg.gps[2].altitude = 0
    # 初始lat：47.397742，lon：8.545593,alt:535.365917
    msg.gps[3].latitude = 0
    msg.gps[3].longitude = 0
    msg.gps[3].altitude = 0

    # Filling PoseStamped (replace with actual pose values)
    msg.pos = PoseStamped()
    msg.pos.pose.position.x = 1.0
    msg.pos.pose.position.y = 2.0
    msg.pos.pose.position.z = 3.0

    msg.task = 3.0


    # msg.gps[0].latitude = 21.679509
    # msg.gps[0].longitude = 110.918809
    # msg.gps[0].altitude = 30.0
    # # 47°23'51.4"N 8°32'52.2"E
    # msg.gps[1].latitude = 0
    # msg.gps[1].longitude = 0
    # msg.gps[1].altitude = 0
    # # 47°23'51.9"N 8°32'44.1"E
    # msg.gps[2].latitude = 0
    # msg.gps[2].longitude =  0
    # msg.gps[2].altitude = 0
    # # 初始lat：47.397742，lon：8.545593,alt:535.365917
    # msg.gps[3].latitude = 0
    # msg.gps[3].longitude = 0
    # msg.gps[3].altitude = 0

    # # Filling PoseStamped (replace with actual pose values)
    # msg.pos = PoseStamped()
    # msg.pos.pose.position.x = 1.0
    # msg.pos.pose.position.y = 2.0
    # msg.pos.pose.position.z = 3.0

    # msg.task = 1.0

    # Filling boolean flag True  False
    msg.doit = True
    # msg.doit = False

    while not rospy.is_shutdown():
        msg.header.stamp = rospy.Time.now()  # Update timestamp
        if should_publish == True:
            pub.publish(msg)
            rospy.loginfo("lat: " + str(msg.gps[0].latitude) + "，lon: " + str(msg.gps[0].longitude) + "，alt: " + str(msg.gps[0].altitude))
        else :
            rospy.loginfo("stop to publish")
        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('flydatas_pub_sub_node', anonymous=True)
    pub = rospy.Publisher('/pub_flydatas', Flydatas, queue_size=10)
    rospy.Subscriber("/sub_flydatas", Flydatas, flydatas_callback)
    
    try:
        publish_flydatas()
    except rospy.ROSInterruptException:
        pass
